/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Pedro Patron
*
*********************************************************************/

#ifndef _OSLCORE_LOCAL_COORD_CONVERTER_H_
#define _OSLCORE_LOCAL_COORD_CONVERTER_H_

#include <iostream>

#include <osl_core/Matrix.h>
#include <osl_core/GlobalCoordConverter.h>

namespace osl_core {

/**
 * Vehicle centered coordinates
 */
typedef struct VCS
{
	double u;		/*!< surge, vehicle centered coordinate system X */
	double v;		/*!< sway, vehicle centered coordinate system Y */
	double w;		/*!< heave, vehicle centered coordinate system Z */
} T_VCS;

/**
 * Vehicle centered speed
 */
typedef struct dVCS
{
	double du;		/*!< surge velocity */
	double dv;		/*!< sway velocity */
	double dw;		/*!< heave velocity */
} T_dVCS;

/**
 * Vehicle centered thrust
 */
typedef struct dVCT
{
	double u;		/*!< surge thrust */
	double v;		/*!< sway thrust */
	double w;		/*!< heave thrust */
} T_VCT;

/**
 * Vehicle attitude coordinates
 */
typedef struct RPY
{
	double r;		/*!< roll */
	double p;		/*!< pitch */
	double y;		/*!< yaw */
} T_RPY;

/**
 * Vehicle angular velocities
 */
typedef struct PQR
{
	double p;		/*!< roll velocity */
	double q;		/*!< pitch velocity  */
	double r;		/*!< yaw velocity */
} T_PQR;

/**
 * Vehicle angular thrust
 */
typedef struct RPYT
{
	double r;		/*!< roll thrust*/
	double p;		/*!< pitch thrust*/
	double y;		/*!< yaw thrust*/
} T_RPYT;

#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, VCS & vcs);
std::ostream & operator << (std::ostream & out, dVCS & dvcs);
std::ostream & operator << (std::ostream & out, dVCT & dvct);
std::ostream & operator << (std::ostream & out, RPY & rpy);
std::ostream & operator << (std::ostream & out, PQR & pqr);
std::ostream & operator << (std::ostream & out, RPYT & rpyt);
#else
ostream & operator << (ostream & out, VCS & vcs);
ostream & operator << (ostream & out, dVCS & dvcs);
ostream & operator << (ostream & out, dVCT & dvct);
ostream & operator << (ostream & out, RPY & rpy);
ostream & operator << (ostream & out, PQR & pqr);
ostream & operator << (ostream & out, RPYT & rpyt);
#endif


// ===========================================================================
/**
 *	Local coordinate systems converter
 *
 *	Converter from local to global coordinates. Angles are stored in radians but
 *	some functions allow accessing class fields in degres by a previous conversion.
 *
 *	References:
 *	- Thor I. Fossen;"Guidance and control of ocean vehicles"; John Wiley & Sons, 1994
 */
class LocalCoordConverter
{
	public:

		/**
		 * Constructor
		 */
		LocalCoordConverter ();
		
		/**
		 * Constructor
		 * @param	originNED	vehicle reference point in NED coordinates
		 * @param	attitude	vehicle attitude: roll, pitch, yaw
		 */
		LocalCoordConverter (const T_NED & origin, const T_RPY & attitude);
						
		/**
		 * Destructor
		 */
		~LocalCoordConverter ();

		/**
		 * @return true if the vehicle reference has been initialised, false otherwise
		 */
		bool		isInitialised();

		/**
		 * Update the vehicle reference system by using a NED point
		 * @param	originNED	the new reference point in NED coordinates
		 * @param	attitude	vehicle attitude in degres: roll, pitch, yaw
		 */
		void 		setReferenceDeg (const T_NED & originNED, const T_RPY & attitude);

		/**
		 * Update the vehicle reference system by using a NED point
		 * @param	originNED	the new reference point in NED coordinates
		 * @param	attitude	vehicle attitude in radians: roll, pitch, yaw
		 */
		void 		setReferenceRad (const T_NED & originNED, const T_RPY & attitude);
		
		/**
		 * @return the reference point in NED coordinates
		 */
		T_NED 		getNEDReference	(void);

		/**
		 * @return the attitude reference in RPY coordinates expresed in radians
		 */
		T_RPY 		getRPYDegReference	(void);

		/**
		 * @return the attitude reference in RPY coordinates expresed in degres
		 */
		T_RPY 		getRPYRadReference	(void);
		
		/**
		 * Converts East, North, Up to Vehicle coordinates
		 * @param	enu	East, North, Up coordinates
		 * @return	point representation in Vehicle coordinates
		 */
		T_VCS		ENU2VCS (const T_ENU & enu);
		
		/**
		 * Converts North,East,Down to Vehicle coordinates
		 * @param	ned	North,East,Down coordinates
		 * @return	point representation in Vehicle coordinates
		 */
		T_VCS		NED2VCS (const	T_NED &	ned ) ;

		/**
		 * Converts Vehicle coordinates to North,East,Down coordinates
		 * @param	vcs	Vehicle coordinates
		 * @return	point representation in North,East,Down coordinates
		 * 
		 * Conversion equation:
		 * \f$\left(\begin{array}{llll}
		 *
		 *  	- \sin yaw * \cos roll + \cos yaw * \sin pitch * \sin roll & 
		 *  	\cos yaw * \cos roll + \sin roll * \sin pitch * \sin yaw &
		 *  	\cos pitch * \sin roll &
		 *  	ned_0.n \\
		 *  	
		 *  	\cos yaw * \cos pitch & 
		 *  	\sin yaw * \cos pitch &
		 *  	- \sin pitch &
		 *  	ned_0.e \\
		 *  	
		 *  	- \sin yaw * \sin roll - \cos yaw * \cos roll * \cos pitch &
		 *  	- \cos yaw * \sin roll - \sin pitch * \sin yaw * \cos roll & 
		 *  	- \cos pitch * \cos roll &
		 *  	ned_0.d \\
		 *  	
		 *  	0 & 0 & 0 & 1 \\
		 *  \end{array}
		 *  \right)
		 *  \left(
		 *  \begin{array}{l}
		 *  vcs.u \\
		 *  vcs.v \\ 
		 *  vcs.w \\
		 *  1
		 *  \end{array}
		 *  \right)
		 *  =
		 *  \left(
		 *  \begin{array}{l}
		 *  ned.n \\
		 *  ned.e \\
		 *  ned.d \\
		 *  1 \\
		 *  \end{array}
		 *  \right)
		 * \f$
		 */
		T_NED 	 	VCS2NED (const	T_VCS &	vcs ) ;
	
		/**
		 * Converts Vehicle coordinates to East, North, Down coordinates
		 * @param	vcs	Vehicle coordinates
		 * @return	point representation in East, North, Down coordinates
		 */
		T_ENU 	 	VCS2ENU (const	T_VCS &	vcs ) ;
		
	private:

		/**
		 * Internal operation for setting new values in rotation matrix
		 */
//		void		setRotMatrixValues (void) ;
		void		setMatrixValues (void) ;

		/**
		 * Vehicle reference frame in North,East,Down global coordinates
		 */
		T_NED								m_NEDRef;		

		/**
		 * Vehicle attitude roll, pitch, yaw in radians
		 */
		T_RPY								m_RPYRef;

		/**
		 * Principal rotation matrix
		 */
//		Matrix<double>				m_RotMatrix;

		Matrix<double>				m_RM;
		Matrix<double>				m_tRM;
};

} // namespace

#endif
